﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public class DiscreteAction2D : ActionDescription
  {
    public DiscreteAction2D()
    {
      typeOfAction = TypeOfAction.none;
    }

    public DiscreteAction2D(TypeOfAction typeOfAction)
    {
      this.typeOfAction = typeOfAction;
    }
    public enum TypeOfAction { moveLeft, moveRight, moveUp, moveDown, turnLeft, turnRight, doNothing, none };
    public TypeOfAction typeOfAction;

    public override ActionDescription InvertAction()
    {
      DiscreteAction2D invertedAction = new DiscreteAction2D(TypeOfAction.doNothing);
      switch (typeOfAction)
      {
        case TypeOfAction.moveDown: invertedAction.typeOfAction = TypeOfAction.moveUp; break;
        case TypeOfAction.moveLeft: invertedAction.typeOfAction = TypeOfAction.moveRight; break;
        case TypeOfAction.moveUp: invertedAction.typeOfAction = TypeOfAction.moveDown; break;
        case TypeOfAction.moveRight: invertedAction.typeOfAction = TypeOfAction.moveLeft; break;
        case TypeOfAction.turnLeft: invertedAction.typeOfAction = TypeOfAction.turnRight; break;
        case TypeOfAction.turnRight: invertedAction.typeOfAction = TypeOfAction.turnLeft; break;
        default: invertedAction.typeOfAction = TypeOfAction.doNothing; break;
      }
      return invertedAction;
    }

    public override WorldState Execute(MotionModel motionModel, WorldState oldState)
    {
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
        return ExecutePerfect(motionModel, oldState);
      else
        return ExecuteWithError(motionModel, oldState);
    }

    public override WorldState ExecutePerfect(MotionModel motionModel, WorldState oldState)
    {
      double newPositionX = ((WorldState2D)oldState).Vector.x;
      double newPositionY = ((WorldState2D)oldState).Vector.y;

      if (typeOfAction == TypeOfAction.moveRight) newPositionX += Globals.motionRasterWidth;
      if (typeOfAction == TypeOfAction.moveLeft) newPositionX -= Globals.motionRasterWidth;
      if (typeOfAction == TypeOfAction.moveUp) newPositionY += Globals.motionRasterWidth;
      if (typeOfAction == TypeOfAction.moveDown) newPositionY -= Globals.motionRasterWidth;
      return new WorldState2D(newPositionX, newPositionY);
    }

    private WorldState ExecuteWithError(MotionModel motionModel, WorldState oldState)
    {
      Random random = new Random();
      double limit = random.NextDouble();
      double sum = 0.0;
      double newPositionX = ((WorldState2D)oldState).Vector.x;
      double newPositionY = ((WorldState2D)oldState).Vector.y;

      bool finished = false;
      int deviation;
      for (deviation = -1; deviation <= 1 && !finished; deviation += 1)
      {
        if (deviation == -1) sum += 0.1;
        if (deviation == 0) sum += 0.8;
        if (deviation == 1) sum += 0.1;
        if (limit < sum)
        {
          finished = true;
        }
      }

      WorldState2D newState;
      switch (typeOfAction)
      {
        case TypeOfAction.moveDown:
          newState = new WorldState2D((oldState as WorldState2D).Vector.x, (oldState as WorldState2D).Vector.y - deviation * Globals.motionRasterWidth);
          break;
        case TypeOfAction.moveUp:
          newState = new WorldState2D((oldState as WorldState2D).Vector.x, (oldState as WorldState2D).Vector.y + deviation * Globals.motionRasterWidth);
          break;
        case TypeOfAction.moveLeft:
          newState = new WorldState2D((oldState as WorldState2D).Vector.x - deviation * Globals.motionRasterWidth, (oldState as WorldState2D).Vector.y);
          break;
        case TypeOfAction.moveRight:
          newState = new WorldState2D((oldState as WorldState2D).Vector.x + deviation * Globals.motionRasterWidth, (oldState as WorldState2D).Vector.y);
          break;
        default:
          newState = new WorldState2D((oldState as WorldState2D).Vector.x, (oldState as WorldState2D).Vector.y);
          break;
      }

      newPositionX = newState.Vector.x;
      newPositionY = newState.Vector.y;

      if (newPositionX < Globals.stateSpaceMinimumX)
        newPositionX = Globals.stateSpaceMinimumX;
      if (newPositionX > Globals.stateSpaceMaximumX)
        newPositionX = Globals.stateSpaceMaximumX - (Globals.stateSpaceMaximumX - Globals.stateSpaceMinimumX) % Globals.motionRasterWidth;

      if (newPositionY < Globals.stateSpaceMinimumY)
        newPositionY = Globals.stateSpaceMinimumY;
      if (newPositionY > Globals.stateSpaceMaximumY)
        newPositionY = Globals.stateSpaceMaximumY - (Globals.stateSpaceMaximumY - Globals.stateSpaceMinimumY) % Globals.motionRasterWidth;


      return new WorldState2D(newPositionX, newPositionY);
    }


    public WorldState ExecuteWithErrorOld(MotionModel motionModel, WorldState oldState)
    {
      Random random = new Random();
      double limit = random.NextDouble();
      double sum = 0.0;
      double newPositionX = ((WorldState2D)oldState).Vector.x;
      double newPositionY = ((WorldState2D)oldState).Vector.y;

      bool finished = false;
      for (double deviationX = -2 * Globals.motionRasterWidth; deviationX <= 2 * Globals.motionRasterWidth && !finished; deviationX += Globals.motionRasterWidth)
      {
        for (double deviationY = -2 * Globals.motionRasterWidth; deviationY <= 2 * Globals.motionRasterWidth && !finished; deviationY += Globals.motionRasterWidth)
        {
          WorldState2D newState = new WorldState2D((oldState as WorldState2D).Vector.x + deviationX, (oldState as WorldState2D).Vector.y + deviationY);
          double probability = motionModel.GetStateTransitionProbability(oldState, newState, this);
          sum += probability;
          if (limit < sum)
          {
            newPositionX = newState.Vector.x;
            newPositionY = newState.Vector.y;
            finished = true;
          }
        }
      }

      if (newPositionX < Globals.stateSpaceMinimumX)
        newPositionX = Globals.stateSpaceMinimumX;
      if (newPositionX > Globals.stateSpaceMaximumX)
        newPositionX = Globals.stateSpaceMaximumX - (Globals.stateSpaceMaximumX - Globals.stateSpaceMinimumX) % Globals.motionRasterWidth;

      if (newPositionY < Globals.stateSpaceMinimumY)
        newPositionY = Globals.stateSpaceMinimumY;
      if (newPositionY > Globals.stateSpaceMaximumY)
        newPositionY = Globals.stateSpaceMaximumY - (Globals.stateSpaceMaximumY - Globals.stateSpaceMinimumY) % Globals.motionRasterWidth;


      return new WorldState2D(newPositionX, newPositionY);
    }
    public override string ToString()
    {
      return
        this.GetType().Name + "|" +
        typeOfAction.ToString();
    }

    public override ActionDescription Parse(string str)
    {
      for (int i = 0; i <= (int)TypeOfAction.none; i++)
        if (((TypeOfAction)i).ToString() == str)
          return new DiscreteAction2D((TypeOfAction)i);
      return new DiscreteAction2D(TypeOfAction.none);
    }
  }
}
